// Testing the pgr_stereo node
// display the captured/streaming images
// Author: Minghao Ruan
// Version: March 3rd

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/CvBridge.h>

#define WFLAGS  (CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO) 
 
void imageCallbackL(const sensor_msgs::ImageConstPtr& msg)
{
  sensor_msgs::CvBridge bridge;
  try
  {
    cvShowImage("view_left", bridge.imgMsgToCv(msg, "bgr8"));
    cvSaveImage("left_image",bridge.imgMsgToCv(msg, "bgr8"));
  }
  catch (sensor_msgs::CvBridgeException& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

void imageCallbackR(const sensor_msgs::ImageConstPtr& msg)
{
  sensor_msgs::CvBridge bridge;
  try
  {
    cvShowImage("view_right", bridge.imgMsgToCv(msg, "bgr8"));
    cvSaveImage("right_image",bridge.imgMsgToCv(msg, "bgr8"));
  }
  catch (sensor_msgs::CvBridgeException& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

using namespace std;
using namespace image_transport;
int main(int argc, char **argv)
{
  ros::init(argc, argv, "image_listener");
  ros::NodeHandle nh, pn("~");
  cvNamedWindow("view_left", WFLAGS);
  cvNamedWindow("view_right",WFLAGS);
  cvStartWindowThread();
  ImageTransport it(nh);

  string stereo, image;
  pn.param<string>("stereo", stereo, "stereo");
  pn.param<string>("image", image, "image_raw");

  string left = stereo+"/left/"+image;
  string right = stereo+"/right/"+image;
  Subscriber subL = it.subscribe(left.c_str(), 1, imageCallbackL);
  Subscriber subR = it.subscribe(right.c_str(),1, imageCallbackR);

  ros::spin();
  cvDestroyWindow("view");
}
